#!/usr/bin/env python

""" detector.py - Version 0.1 2011-06-08

    Mirrored after the ROS C++ REIN Detector operator

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2011 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

import roslib
roslib.load_manifest('pi_rein')
import rospy
import cv
import sys
from pi_rein.msg import *
from pi_rein.srv import *
from sensor_msgs.msg import RegionOfInterest, Image, PointCloud2
from std_msgs.msg import ColorRGBA
from cv_bridge import CvBridge, CvBridgeError
from math import sqrt, isnan
import random

class Detector:
    def __init__(self, node_name):
        
        rospy.init_node(node_name)
        self.node_name = node_name
        
        self.use_display = rospy.get_param("~use_display", True)
        self.show_labels = rospy.get_param("~show_labels", True)
        self.show_features = rospy.get_param("~show_features", True)
        self.show_roi = rospy.get_param("~show_roi", True)
        self.use_image = rospy.get_param("~use_image", True)
        self.use_depth_image = rospy.get_param("~use_depth_image", False)
        self.use_point_cloud = rospy.get_param("~use_point_cloud", False)
        self.use_masks = rospy.get_param("~use_masks", True)
        
        """ Initialize a few global variables used by the REIN framework """
        self.image = None
        self.depth_image = None
        self.point_cloud = PointCloud2()
        self.detections = DetectionArray()
        self.input_detections = DetectionArray()
        self.masks = MaskArray()
        #self.model_storage = ModelStorage()
        
        """ Keep track of the image time stamp so we can synchronize markers in the display_image node """
        self.image_stamp = rospy.Time.now()
        
        """ The ROS to OpenCV bridge object """
        self.cv_bridge = CvBridge()
        
        """ Subscribe to the appropriate topics """
        if self.use_image:
            self.image_sub = rospy.Subscriber("image", Image, self.image_callback)
            
        if self.use_depth_image:
            self.depth_sub = rospy.Subscriber("depth_image", Image, self.depth_image_callback)
            
        if self.use_point_cloud:
            self.point_cloud_sub = rospy.Subscriber("point_cloud", Image, self.point_cloud_callback)
            
        if self.use_masks:
            self.masks_sub = rospy.Subscriber("masks", MaskArray, self.setMasks)
#        else:
#            self.masks = MaskArray()
#            mask = Mask()
#            mask.roi = Rect(0, 0, self.image_width, self.image_height)
#            self.masks.masks.append(mask)
#            self.masks.header.stamp = self.image_stamp
#            self.masks.header.frame_id = '/openni_rgb_optical_frame'

        if self.use_display:
            rospy.wait_for_service('/display_image/draw_markers') 
            self.draw_markers = rospy.ServiceProxy('/display_image/draw_markers', DrawMarkerArray, True)
        
        rospy.loginfo("Starting " + self.node_name)
                
    def image_callback(self, ros_image):
        (self.image, self.image_size, self.image_width, self.image_height) = self.ros2cv_image(ros_image, "bgr8")
    
    def depth_image_callback(self, ros_image):
        (self.depth_image, self.depth_image_size, self.depth_image_width, self.depth_image_height) = self.ros2cv_image(ros_image, "32FC1")
        
    def point_cloud_callback(self, point_cloud):
        pass
        
    def ros2cv_image(self, ros_image, type):
        try:
            cv_image = self.cv_bridge.imgmsg_to_cv(ros_image, type)
            size = cv.GetSize(cv_image)
            width = size[0]
            height = size[1]
            return (cv_image, size, width, height)
        except CvBridgeError, e:
          print e
    
    def getDetections(self):
        return self.detections
    
    def setInputDetections(self, input_detections):
        self.input_detections = input_detections
                
    def getName(self):
        return self.node_name
        
    def detect(self):
        pass
    
    def setImage(self, image):
        self.image = image
        
    def setDepthImage(self, depth_image):
        self.depth_image = depth_image
    
    def setMasks(self, masks):
        #rospy.loginfo(masks.masks[0].roi)
        self.masks = masks
    
    def setPointCloud(self, point_cloud):
        self.point_cloud = point_cloud
    
    def loadModels(self):
        pass
    

def main(args):
    # Display a help message if appropriate.
    help_message = ""
          
    print help_message
    
    try:
        detector = Detector("test_detector")
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down detector node."

if __name__ == '__main__':
    main(sys.argv)